//
// Created by lqk on 2022/2/21.
//

#include "PyBulletIMUDriver.h"

IMUInfo g_IMUInfo;

void PyBulletIMUDriver::Init() {
    if(!m_lcmSub.good()) std::cout <<"LCM subscriber not good!"<<std::endl;
    m_lcmSub.subscribe(m_subChannel.c_str(),&PyBulletIMUDriver::Callback,this);
}

void PyBulletIMUDriver::RunOnce() {
    m_lcmSubHandle = m_lcmSub.handleTimeout(500);
}

void PyBulletIMUDriver::CleanUp() {

}

void PyBulletIMUDriver::Callback(const lcm::ReceiveBuffer *rbuf, const std::string &channel,
                                 const quad_bot::IMU_info_pb *msg) {


}

void PyBulletIMUDriver::PrintStatus() {
    std::cout<<m_name<<" is running!"<<std::endl;
    if(m_lcmSubHandle==0){
        std::cout<<m_name<<" subscription time out!"<<std::endl;
    }
}